#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Hubs,  S4, HTMotor,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     HTSMUX,         sensorI2CCustom)
#pragma config(Sensor, S3,     HTGYRO,         sensorAnalogInactive)
#pragma config(Sensor, S4,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     Left,          tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     Rear,          tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     Right,         tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     Front,         tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     Sweeper,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     Redirect,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_1,     Lift,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_2,     motorK,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    BallRedirect,         tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    Hook,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    CenterGoal,           tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    KickArm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "TeleopHeaderDice.h"

task main()
{
	initializeRobot();
	getJoystickSettings(joystick);
	while (true)
	{
		//writeDebugStreamLine("Current dist: %d", HTEOPDreadRaw(HTEOPD));
		//printValues();
		joystick1_x = joystick.joy1_x1 * JOYSTICK_SCALE;
		joystick1_y = joystick.joy1_y1 * JOYSTICK_SCALE;
		joystick2_x = joystick.joy1_x2 * JOYSTICK_SCALE;
		joystick2_y = joystick.joy1_y2 * JOYSTICK_SCALE;

		if (areJoysticksCentered(joystick1_x,joystick1_y,joystick2_x))
		{
			halt();
		}
		else
		{
			movement(-joystick1_x, joystick1_y, -joystick2_x, slowMode);
		}

		switch (joystick.joy1_TopHat)//Control for center goal scoring.
		{
			case -1:
			{
				motor[Lift] = 0;
			}
			//case 2:
			//{
			//	motor[Lift] = 100;
			//	break;
			//}
			//case 4:
			//{
			//	servo[CenterGoal] = CENTER_GOAL_DOWN;
			//	break;
			//}
			//case 6:
			//{
			//	motor[Lift] = -50;
			//	break;
			//}
			default:
			{
				motor[Lift] = 0;
				break;
			}
		}
		if (joy1Btn(1) == 1)//Toggles hook servo on button press
		{
			if (!hookButtonPressed)
			{
				toggleHook();
				hookButtonPressed = true;
			}
		}
		else
		{
			hookButtonPressed = false;
		}

		if (joy1Btn(5) == 0)
		{
			slowMode = 1;
		}
		else
		{
			slowMode = 0.5;
		}

		//	if (joy1Btn(4) == 1)//Runs the sweeper in reverse; ONLY USE WHEN NECESSARY.
		//{
		//	if (!isSweeperRunning)
		//	{

		//		motor[Sweeper] = -100;
		//	}

		//}
		//else
		//{
		//	if (!isSweeperRunning)
		//	{
		//		motor[Sweeper] = 0;
		//	}
		//}
		if (joy1Btn(4) == 1)
		{
				motor[Right] = 30;
				motor[Left] = -30;
				motor[Rear] = 30;
				motor[Front] = -30;
				time1(T1) = 0;
				while ((joy1Btn(4) == 1 && HTEOPDreadProcessed(HTEOPD) < 20) && time1(T1) < 5000)
				{
				}
				halt();
				servo[Hook] = SERVO_LEFT_DOWN;
		}

		if (joy1Btn(6) == 1)//Toggles kick servo on button press
		{
			if (!armButtonPressed)
			{
				toggleKick();
				armButtonPressed = true;
			}
		}
		else
		{
			armButtonPressed = false;
		}

		if (joy1Btn(8) == 1)//Raises the ball redirect
		{
			motor[Redirect] = -70;
		}
		else if (joy1Btn(7) == 1)//Lowers the ball redirect
		{
			motor[Redirect] = 70;
		}
		else
		{
			motor[Redirect] = 0;
		}

	}//end while loop

}// end of main
